using System.Drawing;
using Microsoft.Ccr.Core;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using Microsoft.Robotics.PhysicalModel;
using physProxy = Microsoft.Robotics.PhysicalModel.Proxy;
using phys = Microsoft.Robotics.PhysicalModel;
using W3C.Soap;
using gridslam = Diversity.Robotics.Navigation.GridSlam;
using ds = Microsoft.Dss.Services.Directory;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using lrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
using sonar = Microsoft.Robotics.Services.Sonar.Proxy;
using contact = Microsoft.Robotics.Services.ContactSensor.Proxy;

namespace Diversity.Robotics.Navigation.GridSlamService
{
    
    /// <summary>
    /// Implementation class for the GridSlamService
    /// </summary>
    [DisplayName("GridSlamService")]
    [Description("The GridSlam Service")]
    [Contract(Contract.Identifier)]
    public class GridSlamService : DsspServiceBase
    {
        // temporary hack
        private static readonly double _mapMiddle = 750;
        private bool allowProcessing = true;

        // private storage
        private GridSlam _gridSlam;
        private Robot _robot;
        private List<IConeModel> _coneModels;
        private List<BeamModel> _beamModels;

        /// <summary>
        /// Stores all the settings for grid slam
        /// </summary>
        private GridSlamState _state;
        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/gridslam", AllowMultipleInstances=false)]
        private GridSlamOperations _mainPort = new GridSlamOperations();


        GridSlamForm _gridSlamForm;
        GridSlamFormEvents _eventsPort = new GridSlamFormEvents();

        private Port<ProcessData> _internalSlamPort = new Port<ProcessData>();
        private Port<OnRequestMapUpdate> _internalImageRequestPort = new Port<OnRequestMapUpdate>();

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public GridSlamService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
			//base.Start();

            Prepare();

            Activate(Arbiter.Interleave(
             new TeardownReceiverGroup
             (
                 Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DropHandler)
             ),
             new ExclusiveReceiverGroup
             (
                 Arbiter.ReceiveWithIterator<Replace>(true, _mainPort, ReplaceHandler),
                 Arbiter.ReceiveWithIterator<OnLoad>(true, _eventsPort, OnLoadHandler),
                 Arbiter.Receive<OnClosed>(true, _eventsPort, OnClosedHandler),
                 Arbiter.ReceiveWithIterator<OnNewBeamModel>(true, _eventsPort, OnNewBeamModelHandler),
                 Arbiter.ReceiveWithIterator<OnNewConeModel>(true, _eventsPort, OnNewConeModelHandler),
                 Arbiter.ReceiveWithIterator<OnNewSensorType>(true, _eventsPort, OnNewSensorTypeHandler),
                 Arbiter.ReceiveWithIterator<OnNewSensor>(true, _eventsPort, OnNewSensorHandler),
                 Arbiter.ReceiveWithIterator<OnClearMaps>(true, _eventsPort, OnClearMapsHandler),
                 Arbiter.ReceiveWithIterator<OnUpdateOdometryModel>(true, _eventsPort, OnUpdateOdometryModelHandler)
             ),
             new ConcurrentReceiverGroup
             (
                 Arbiter.Receive<DsspDefaultLookup>(true, _mainPort, DefaultLookupHandler),
                 Arbiter.ReceiveWithIterator<Get>(true, _mainPort, GetHandler),
                 Arbiter.ReceiveWithIterator<OnConnect>(true, _eventsPort, OnConnectHandler),
                 Arbiter.ReceiveWithIterator<OnConnectDrive>(true, _eventsPort, OnConnectDriveHandler),
                 Arbiter.ReceiveWithIterator<OnConnectLRF>(true, _eventsPort, OnConnectLRFHandler),
                 Arbiter.ReceiveWithIterator<drive.Update>(true, _driveNotify, OnDriveUpdateNotificationHandler),
                 Arbiter.ReceiveWithIterator<lrf.Replace>(true, _laserNotify, OnLaserReplaceHandler),
                 Arbiter.ReceiveWithIterator<OnRequestMapUpdate>(true, _eventsPort, OnRequestMapUpdateHandler),
                 Arbiter.ReceiveWithIterator<OnRequestBeamModelImage>(true, _eventsPort, OnRequestBeamModelImageHandler)
             )
         ));

            // *******************************************************************************************
            Dispatcher dispatcher1 = new Dispatcher(0, "GridSlamStepDispatcher");
            DispatcherQueue depthThrottledQueue1 = new DispatcherQueue("ConstrainQueueDepthDiscard1",
                  dispatcher1,
                  TaskExecutionPolicy.ConstrainQueueDepthDiscardTasks,
                  1);

            Arbiter.Activate(depthThrottledQueue1,
                             Arbiter.ReceiveWithIterator(true, _internalSlamPort, processSlamDataHandler));
            // *******************************************************************************************

            // *******************************************************************************************
            Dispatcher dispatcher2 = new Dispatcher(0, "GridSlamImageDispatcher");
            DispatcherQueue depthThrottledQueue2 = new DispatcherQueue("ConstrainQueueDepthDiscard2",
                  dispatcher2,
                  TaskExecutionPolicy.ConstrainQueueDepthDiscardTasks,
                  1);

            Arbiter.Activate(depthThrottledQueue2,
                             Arbiter.ReceiveWithIterator(true, _internalImageRequestPort, InternalOnRequestMapUpdateHandler));
            // *******************************************************************************************
 

            DirectoryInsert();
            LogInfo("GridSlam Start");
            WinFormsServicePort.Post(new RunForm(CreateForm));
        }

        #region WinForms interaction

        System.Windows.Forms.Form CreateForm()
        {
            LogInfo("GridSlam CreateForm");
            return new GridSlamForm(_eventsPort);
        }

        IEnumerator<ITask> OnLoadHandler(OnLoad onLoad)
        {
            LogInfo("GridSlam OnLoad");
            _gridSlamForm = onLoad.GridSlamForm;

            WinFormsServicePort.FormInvoke(delegate()
            {
                _gridSlamForm.UpdateFromState(_state);
            });

            yield break;
        }

        void OnClosedHandler(OnClosed onClosed)
        {
            if (onClosed.GridSlamForm == _gridSlamForm)
            {
                LogInfo("Form Closed");
                _mainPort.Post(new DsspDefaultDrop(DropRequestType.Instance));
            }
        }

        IEnumerator<ITask> OnConnectHandler(OnConnect onConnect)
        {
           
            if (onConnect.GridSlamForm == _gridSlamForm)
            {
                UriBuilder builder = new UriBuilder(onConnect.Service);
                builder.Scheme = new Uri(ServiceInfo.Service).Scheme;

                ds.DirectoryPort port = ServiceForwarder<ds.DirectoryPort>(builder.Uri);
                ds.Get get = new ds.Get();

                port.Post(get);
                ServiceInfoType[] list = null;

                yield return Arbiter.Choice(get.ResponsePort,
                    delegate(ds.GetResponseType response)
                    {
                        LogInfo("GridSlam OnConnect response");
                        list = response.RecordList;
                    },
                    delegate(Fault fault)
                    {
                        list = new ServiceInfoType[0];
                        LogError(fault);
                    }
                );

                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        _gridSlamForm.ReplaceServicesList(list);
                    }
                );
            }
        }

        IEnumerator<ITask> OnNewConeModelHandler(OnNewConeModel onNewConeModel)
        {
            if (onNewConeModel.GridSlamForm == _gridSlamForm)
            {
                ConeModelState cms = new ConeModelState();
                cms.Name = "New Cone Model #" + _state.ConeModelStates.Count;
                cms.Id = _state.ConeModelStates.Count;
                _state.ConeModelStates.Add(cms);
                

                // tell the form to update
                WinFormsServicePort.FormInvoke(delegate()
               {
                   _gridSlamForm.UpdateFromState(_state);
               });
            }
            yield break;
        }

        IEnumerator<ITask> OnNewBeamModelHandler(OnNewBeamModel onNewBeamModel)
        {
            if (onNewBeamModel.GridSlamForm == _gridSlamForm)
            {
                BeamModelState bms = new BeamModelState();
                bms.Name = "New Beam Model #" + _state.BeamModelStates.Count;
                bms.Id = _state.BeamModelStates.Count;
                _state.BeamModelStates.Add(bms);
                _beamModels.Add(new BeamModel(bms.HitVariance, bms.ShortDecay, bms.MaxRange, bms.MixHit, bms.MixShort, bms.MixRandom, bms.MixFailure));
                
                // tell the form to update
                WinFormsServicePort.FormInvoke(delegate()
               {
                   _gridSlamForm.UpdateFromState(_state);
               });
            }
            yield break;
        }


        IEnumerator<ITask> OnNewSensorTypeHandler(OnNewSensorType onNewSensorType)
        {
            if (onNewSensorType.GridSlamForm == _gridSlamForm)
            {
                SensorTypeState sts = new SensorTypeState();
                sts.Name = "New Sensor Type #" + _state.SensorTypes.Count;
                sts.Id = _state.SensorTypes.Count;
                sts.ConeModelId = 0;
                sts.ConeModelId = 0;
                _state.SensorTypes.Add(sts);

                // tell the form to update
                WinFormsServicePort.FormInvoke(delegate()
               {
                   _gridSlamForm.UpdateFromState(_state);
               });
            }
            yield break;
        }

        IEnumerator<ITask> OnNewSensorHandler(OnNewSensor onNewSensor)
        {
            if (onNewSensor.GridSlamForm == _gridSlamForm)
            {
                SensorState ss = new SensorState();
                ss.Name = "New Sensor #" + _state.SensorTypes.Count;
                ss.Id = _state.SensorTypes.Count;
                ss.SensorTypeId = 0;
               
                _state.RobotState.Sensors.Add(ss);

                // tell the form to update
                WinFormsServicePort.FormInvoke(delegate()
               {
                   _gridSlamForm.UpdateFromState(_state);
               });
            }
            yield break;
        }

        Choice PerformShutdown(ref Port<Shutdown> port)
        {
            Shutdown shutdown = new Shutdown();
            port.Post(shutdown);
            port = null;

            return Arbiter.Choice(
                shutdown.ResultPort,
                delegate(SuccessResult success) { },
                delegate(Exception e)
                {
                    LogError(e);
                }
            );
        }

        IEnumerator<ITask>  OnUpdateOdometryModelHandler(OnUpdateOdometryModel onUpdateOdometryModel)
        {
            if (onUpdateOdometryModel.GridSlamForm == _gridSlamForm)
            {
                _state.RobotState.MotionModel.VErr1 = onUpdateOdometryModel.VErr1;
                _state.RobotState.MotionModel.VErr2 = onUpdateOdometryModel.VErr2;
                _state.RobotState.MotionModel.RErr1 = onUpdateOdometryModel.RErr1;
                _state.RobotState.MotionModel.RErr1 = onUpdateOdometryModel.RErr2;

                OdometryMotionModel odo = (OdometryMotionModel) _gridSlam.Robot.MotionModel;
                odo.tErr1 = onUpdateOdometryModel.VErr1;
                odo.tErr2 = onUpdateOdometryModel.VErr2;
                odo.rErr1 = onUpdateOdometryModel.RErr1;
                odo.rErr1 = onUpdateOdometryModel.RErr2;

                _gridSlam.Robot.MotionModel = odo;
            }
            yield break;
        }
        #endregion

        #region Prepare default state or load
        private void Prepare()
        {
            if (_state == null)
            {
                // initialise some state
                BeamModelState bms = new BeamModelState();
                bms.Name = "Sonar Beam Model";
                bms.Id = 0;
                bms.MaxRange = 800;
                bms.HitVariance = 600;
                bms.ShortDecay = 0.1;
                bms.MixHit = 0.8;
                bms.MixRandom = 0.1;
                bms.MixShort = 0.1;
                bms.MixFailure = 0;

                ConeModelState cms = new ConeModelState();
                cms.Name = "Sonar Cone Model";
                cms.Id = 0;
                cms.MaxRange = 800;
                cms.ObstacleSize = 20;
                cms.ConeWidth = 0.05;
                
                SensorTypeState sts = new SensorTypeState();
                sts.Name = "Sonar";
                sts.Id = 0;
                sts.BeamModelId = 0;
                sts.ConeModelId = 0;
                sts.BeamModelName = "Sonar Beam Model";
                sts.ConeModelName = "Sonar Cone Model";

                RobotState rs = new RobotState();
                rs.Width = 20;
                rs.Length = 25;
                rs.Pose2DBestEstimate = new Pose2DState(_mapMiddle, _mapMiddle, 0);
                rs.Pose2DIncoming = new Pose2DState(_mapMiddle, _mapMiddle, 0);
                rs.MotionModel = new MotionModelState(0.00004,0.1,0.01,0.01);
                
                SensorState ss = new SensorState();
                ss.Name = "Front Sonar";
                ss.Id = 0;
                ss.SensorTypeId = 0;
                ss.SensorTypeName = "Sonar";
                ss.X = 0;
                ss.Y = 0;
                ss.Theta = 0;

                rs.Sensors = new List<SensorState>();
                rs.Sensors.Add(ss);

                GridSlamState gss = new GridSlamState();
                gss.RobotState = rs;
                gss.SensorTypes = new List<SensorTypeState>();
                gss.SensorTypes.Add(sts);
                gss.BeamModelStates = new List<BeamModelState>();
                gss.BeamModelStates.Add(bms);
                gss.ConeModelStates = new List<ConeModelState>();
                gss.ConeModelStates.Add(cms);
                gss.MapHeight = 300;
                gss.MapWidth = 300;
                gss.CmPerPixel =5;
                gss.NumParticles = 50;
                
                _state = gss;
                SaveState(gss);
            }
            ReadState();
           
        }

        public void ReadState()
        {
            _coneModels = new List<IConeModel>();
            _beamModels = new List<BeamModel>();

            // prepare the robot
           
            Map globalMap = new Map(_state.MapWidth, _state.MapHeight, 0.6, _state.CmPerPixel);
            _robot = new Robot(_state.RobotState.Pose2DBestEstimate.ToPose2D(), _state.MapWidth, _state.MapHeight, globalMap);
            _robot.MotionModel = new OdometryMotionModel(_state.RobotState.MotionModel.VErr1,
                                     _state.RobotState.MotionModel.VErr2,
                                     _state.RobotState.MotionModel.RErr1,
                                     _state.RobotState.MotionModel.RErr2);

            // create beam model list
            _beamModels = new List<BeamModel>();
            foreach (BeamModelState beamModelState in _state.BeamModelStates)
            {
                BeamModel bm = new BeamModel(beamModelState.HitVariance,beamModelState.ShortDecay,beamModelState.MaxRange,beamModelState.MixHit, beamModelState.MixShort, beamModelState.MixRandom, beamModelState.MixFailure);
                _beamModels.Add(bm);
            }

            // create cone model list
            _coneModels = new List<IConeModel>();
            foreach (ConeModelState coneModelState in _state.ConeModelStates)
            {
                ConeModelLaser coneModel = new ConeModelLaser(coneModelState.MaxRange, coneModelState.ObstacleSize);
             //   ConeModel1Gaussian coneModel = new ConeModel1Gaussian(coneModelState.MaxRange, coneModelState.ConeWidth, 0.1, coneModelState.ObstacleSize, 0.2, 0.2);
                _coneModels.Add(coneModel);
            }
            

            // add some sensors
            foreach (SensorState sensorState in _state.RobotState.Sensors)
            {
                if (sensorState.SensorTypeId >= 0 && sensorState.SensorTypeId < _state.SensorTypes.Count)
                {
                    int beamModelId = _state.SensorTypes[sensorState.SensorTypeId].BeamModelId;
                    int coneModelId = _state.SensorTypes[sensorState.SensorTypeId].ConeModelId;
                    if (beamModelId >=0 && beamModelId < _beamModels.Count)
                    {
                        if (coneModelId >= 0 && coneModelId < _coneModels.Count)
                        {
                            // all good.
                            DistanceSensor ds = new DistanceSensor(sensorState.X,sensorState.Y,sensorState.Theta,_beamModels[beamModelId],_coneModels[coneModelId]);
                            _robot.AddDistanceSensor(ds);
                        }
                        else
                        {
                            Console.Out.WriteLine("GridSlam: SensorType has invalid cone model type.");
                        }
                    } else
                    {
                        Console.Out.WriteLine("GridSlam: SensorType has invalid beam model type.");
                    }
                    
                } else
                {
                    Console.Out.WriteLine("GridSlam: Sensor definition has invalid sensor type.");
                }
                
            }
            _gridSlam = new GridSlam(_robot, _state.NumParticles);
            _gridSlam.DoStepWhenVelocityZero = false;
            _gridSlam.InformationReduction = 1;
            _gridSlam.ConditionSamplesOnMap = true;
        }
        #endregion

        #region Standard Handlers 
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        public virtual IEnumerator<ITask> ReplaceHandler(Replace replace)
        {
            _state = replace.Body;
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// Drop Handler
        /// </summary>
        /// <param name="drop"></param>
        void DropHandler(DsspDefaultDrop drop)
        {
            if (_gridSlamForm != null)
            {
                GridSlamForm drive = _gridSlamForm;
                _gridSlamForm = null;

                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        if (!drive.IsDisposed)
                        {
                            drive.Dispose();
                        }
                    }
                );
            }

            base.DefaultDropHandler(drop);
        }

        #endregion

        #region Motor operations

        drive.DriveOperations _drivePort = null;
        drive.DriveOperations _driveNotify = new drive.DriveOperations();
        Port<Shutdown> _motorShutdown = null;

        IEnumerator<ITask> OnConnectDriveHandler(OnConnectDrive onConnectDrive)
        {
            Console.Out.WriteLine("Received drive connect");
            if (onConnectDrive.GridSlamForm == _gridSlamForm)
            {
                drive.EnableDriveRequest request = new drive.EnableDriveRequest();

                if (_drivePort != null)
                {
                    yield return Arbiter.Choice(
                        _drivePort.EnableDrive(request),
                        delegate(DefaultUpdateResponseType response) { },
                        delegate(Fault f)
                        {
                            LogError(f);
                        }
                    );

                    if (_motorShutdown != null)
                    {
                        yield return PerformShutdown(ref _motorShutdown);
                    }
                }

                _drivePort = ServiceForwarder<drive.DriveOperations>(onConnectDrive.Service);
                _motorShutdown = new Port<Shutdown>();

                
                request.Enable = true;

                yield return Arbiter.Choice(
                    _drivePort.EnableDrive(request),
                    delegate(DefaultUpdateResponseType response) { },
                    delegate(Fault f)
                    {
                        LogError(f);
                    }
                );
                
                drive.ReliableSubscribe subscribe = new drive.ReliableSubscribe(
                    new ReliableSubscribeRequestType(10)
                );
                subscribe.NotificationPort = _driveNotify;
                subscribe.NotificationShutdownPort = _motorShutdown;

                _drivePort.Post(subscribe);

                yield return Arbiter.Choice(
                    subscribe.ResponsePort,
                    delegate(SubscribeResponseType response)
                    {
                        LogInfo("GridSlam Subscribed to " + onConnectDrive.Service);
                    },
                    delegate(Fault fault)
                    {
                        _motorShutdown = null;
                        LogError(fault);
                    }
                );
            }
        }

        IEnumerator<ITask> OnDriveUpdateNotificationHandler(drive.Update notification)
        {
            LogInfo("Received drive update");
            if (_gridSlamForm != null)
            {
                _state.RobotState.Pose2DIncoming = ConvertDiffDriveToPose2DState(notification.Body);
               
                WinFormsServicePort.FormInvoke(
                    delegate()
                    {
                        _gridSlamForm.UpdateDriveData(notification.Body);
                    }
                );
            }

            yield break;
        }

        private static Pose2D ConvertDiffDriveToPose2D(drive.DriveDifferentialTwoWheelState diffDrive)
        {
            // Assume that we are using the differentionDriveTT
            Pose2D leftPose =
                new Pose2D(_mapMiddle - diffDrive.LeftWheel.MotorState.Pose.Position.X * 100,
                           _mapMiddle - diffDrive.LeftWheel.MotorState.Pose.Position.Z * 100, GetYaw(diffDrive.LeftWheel.MotorState.Pose.Orientation));
           // Console.Out.WriteLine("Pose is: " + leftPose);
            return leftPose;
        }

        private static Pose2DState ConvertDiffDriveToPose2DState(drive.DriveDifferentialTwoWheelState diffDrive)
        {
            Pose2D p = ConvertDiffDriveToPose2D(diffDrive);
            return new Pose2DState(p);
        }

        private static double GetYaw(physProxy.Quaternion q)
        {
            double a =  Math.Acos(q.W)*2;
            
            if (q.Y < 0)
            {
                return a;
            } else
            {
                return -a;
            }
               
            /*
             * // jump through hoops, then use overkill
            phys.Quaternion qq = new phys.Quaternion(q.X,q.Y,q.Z,q.W);
            phys.AxisAngle axisAngle = phys.Quaternion.ToAxisAngle(qq);
            double yaw, pitch, roll;
            double x = axisAngle.Axis.X;
            double y = axisAngle.Axis.Y;
            double z = axisAngle.Axis.Z;
            double a = axisAngle.Angle;

            double s = Math.Sin(a);
            double c = Math.Cos(a);
            double t = 1 - c;
            if ((x * y * t + z * s) > 0.998)
            {
                yaw = 2 * Math.Atan2(x * Math.Sin(a / 2), Math.Cos(a / 2));
                
            }
            else if ((x * y * t + z * s) < -0.998)
            {
                yaw = -2 * Math.Atan2(x * Math.Sin(a / 2), Math.Cos(a / 2));
                
            }
            else
            {
                yaw = Math.Atan2(y * s - x * z * t, 1 - (y * y + z * z) * t);
            }
            return yaw;*/

        }

      
        #endregion

        #region Laser Range Finder operations

        lrf.SickLRFOperations _laserPort;
        lrf.SickLRFOperations _laserNotify = new lrf.SickLRFOperations();
        Port<Shutdown> _laserShutdown = null;

        IEnumerator<ITask> OnConnectLRFHandler(OnConnectLRF onConnectSickLRF)
        {
            if (onConnectSickLRF.GridSlamForm != _gridSlamForm)
                yield break;
            _laserPort = ServiceForwarder<lrf.SickLRFOperations>(onConnectSickLRF.Service);
            _laserShutdown = new Port<Shutdown>();

            lrf.ReliableSubscribe subscribe = new lrf.ReliableSubscribe(
                new ReliableSubscribeRequestType(5)
            );
            subscribe.NotificationPort = _laserNotify;
            subscribe.NotificationShutdownPort = _laserShutdown;

            _laserPort.Post(subscribe);

            yield return Arbiter.Choice(
                subscribe.ResponsePort,
                delegate(SubscribeResponseType response)
                {
                    LogInfo("GridSlam Subscribed to " + onConnectSickLRF.Service);
                },
                delegate(Fault fault)
                {
                    _laserShutdown = null;
                    LogError(fault);
                }
            );
        }

        IEnumerator<ITask> OnLaserReplaceHandler(lrf.Replace replace)
        {
            LogInfo("Received Laser Replace" );
            if (_gridSlamForm != null)
            {
             
                // as the lrf state doesn't contain a pose, we have to assume that it is centered
                // TODO unless an offset has been specified

                int readingCount = replace.Body.DistanceMeasurements.Length;
                int currentReading = 0;
                double startAngle = Math.PI;
                double angleStep = -Math.PI/readingCount;

                // for the moment, use the general DistanceReading container
                // in distance sensor 0
                List<DistanceReadingState> readings = new List<DistanceReadingState>();

                foreach (int distanceMeasurement in replace.Body.DistanceMeasurements)
                {
                    // from mm to cm
                    readings.Add(new DistanceReadingState(distanceMeasurement/10,startAngle + (currentReading++ * angleStep),0));
                       
                }
                
                ProcessData pd = new ProcessData(_state.RobotState.Pose2DIncoming, readings);
                _internalSlamPort.Post(pd);
              
                /*
                if (_gridSlamForm != null) { // second check
                    WinFormsServicePort.FormInvoke(
                        delegate()
                        {
                            _gridSlamForm.UpdateLrfData(replace.Body);
                        }
                    );
                }
                 * */
            }

            yield break;
        }

        void OnDisconnectSickLRFHandler(OnDisconnectLRF onDisconnectSickLRF)
        {
            PerformShutdown(ref _laserShutdown);
        }

            #endregion

        #region GridSlam Handlers

        /// <summary>
        /// The main slam step handler
        /// </summary>
        /// <param name="data"></param>
        /// <returns></returns>
         IEnumerator<ITask> processSlamDataHandler (ProcessData data)
         {
             if (allowProcessing) {
             // main slam step
             // convert the distance data to expected types
             List<DistanceReading> readings = new List<DistanceReading>();
            foreach (DistanceReadingState distanceReadingState in data.Body.distanceData)
            {
                readings.Add(_robot.DistanceSensors[distanceReadingState.SensorID].CreateReading(distanceReadingState.Distance,
                                                              distanceReadingState.AngleOffset));
            }
            // DateTime t0 = DateTime.Now;

             _gridSlam.DoStep(data.Body.poseData.ToPose2D(), readings.ToArray());

            // DateTime t1 = DateTime.Now;
            // Console.Out.WriteLine(t1 + " DoStep took: " + TimeSpan.FromTicks(t1.Ticks - t0.Ticks).TotalMilliseconds);

             // update the best estimate
             _state.RobotState.Pose2DBestEstimate = new Pose2DState(_gridSlam.GetBestState());
             }
             yield break;
         }

        IEnumerator<ITask> OnRequestMapUpdateHandler(OnRequestMapUpdate onRequestMapUpdate)
        {
            _internalImageRequestPort.Post(onRequestMapUpdate);
           
            yield break;
        }

        IEnumerator<ITask> InternalOnRequestMapUpdateHandler(OnRequestMapUpdate onRequestMapUpdate)
        {

            if (onRequestMapUpdate.GridSlamForm == _gridSlamForm)
            {
                Image i;
                if (onRequestMapUpdate.MapType == MapType.BestMap)
                {
                    i = _gridSlam.GetBestImage();

                }
                else
                {
                    i = _gridSlam.GetLocalImage();
                }

                WinFormsServicePort.FormInvoke(
                   delegate()
                   {
                       _gridSlamForm.UpdateMap((Bitmap)i);
                   }
               );
            }

            yield break;
        }


        IEnumerator<ITask> OnClearMapsHandler(OnClearMaps onClearMaps)
        {
            allowProcessing = false;
            ReadState();
            allowProcessing = true;
            yield break;
        }
        IEnumerator<ITask> OnRequestBeamModelImageHandler(OnRequestBeamModelImage onRequestBeamModelImage)
        {
            if (onRequestBeamModelImage.GridSlamForm == _gridSlamForm)
            {
                if (onRequestBeamModelImage.Row >= _beamModels.Count)
                {
                    // missing model
                }
                else
                {
                    Image i = new Bitmap(onRequestBeamModelImage.Width, onRequestBeamModelImage.Height);
                    i = _beamModels[onRequestBeamModelImage.Row].GetDistributionImage(i,
                                                                                      _beamModels[
                                                                                          onRequestBeamModelImage.Row].
                                                                                          MaxRange*0.7);

                    WinFormsServicePort.FormInvoke(
                        delegate()
                            {
                                _gridSlamForm.UpdateBeamModelImage((Bitmap) i);
                            }
                        );
                }
            }
            yield break;
        }


        #endregion
    }
}
